clc; clear
% 开始解决第一个问题

A =[2.998806	0.220528
30.323206	15.844528
23.792806	29.414528
-4.748594	12.996528
2.998806	0.220528
] ;
 B =[11.149906	18.121528
-3.120794	9.912528
];
 C =[12.777706	15.037528
-1.492994	6.828528
];
D=[1.380606	2.756528
15.301006	10.696528
14.405506	11.953528
0.134806	3.744528
1.380606	2.7565285
];
 E=[
18.39721694	9.025287575
20.73791694	10.36928757
13.59911694	23.54228757
11.25831694	22.19828757
18.39721694	9.025287575
];
 F=[
6.783106	19.744528
2.529706	17.311528
5.012206	12.971528
9.265606	15.404528
6.783106	19.744528
];
G= [
20.072206	18.822528
13.127906	14.850528
14.071306	13.201528
21.015606	17.173528
20.072206	18.822528
];
figure(3);
plot(A(:,1),A(:,2));hold on;
plot(B(:,1),B(:,2),':r');hold on;
plot(C(:,1),C(:,2),':g');hold on;
plot(D(:,1),D(:,2),':k');hold on;
plot(E(:,1),E(:,2),':m');hold on;
% plot(F(:,1),F(:,2),':m');hold on;
% plot(G(:,1),G(:,2),':m');hold on;
%6.53023	3.970426

h = plotcar(6.53023,3.970426,atand(0.5194235),0); hold on;
h2 = plot(6.53023,1.97043, ':m', 'linewidth',2);

%% 前轮偏转角度函数 fdelta 和后轴中点速度函数 fv 可由控制函数或手动设置得到
% % [fdelta, fv] = autocontrol(bd, amax, vmax, omega, phimax, dir); 
% 前轮最大转动角速度为 max(dbeta) = 400/16 = 25 deg/s，这里取为 20 deg/s
dbeta = 25;   % [deg/s] 前轮转动角速度，可增大至 25
maxalf = 470/16;
tmax = 470/16/dbeta; % 前轮最大偏转角为 470/16 度，则转动时间为 470/16/dbeta
zalog = atand(0.5194235);% 车身初始角度

re= backrad([2.998806	0.220528],[30.323206	15.844528]);% 墙布角度
zalog2 =re;

re2 = re+190;
zalog3 = 222.2917;
re3 =  backrad([-3.120794	9.912528],[11.149906	18.121528])+180;

vec = 0.5;% 普通速度
l=2.8;% 车身长
alpa = (re-zalog);
womiga = rad2deg(vec*tand(maxalf)/l);
%  womiga = vec*(maxalf)/l;

t2 = (alpa-6.278486626882780)/womiga;

the_max_error = 6.2785;
% ala=5;
% wegalf = deg2rad(ala);
% dbeta1=deg2rad(dbeta);
% rotefunction =@(x)exp(-wegalf*l*dbeta1/(2*vec))-cos(dbeta1*x);
% daofun = @(x) dbeta1*sin(dbeta1*x);
% tort=acos(exp(-wegalf*l*dbeta1/(2*vec)))/dbeta1;
% %tort = newiteration(rotefunction,daofun,0.5,0.00000001);
% disp(tort);
% tzero = 0;
time =adjust_car_angle(vec,25,zalog,re);
t2t = 10;
tt4 = 0.2235;
time1 =adjust_car_angle(vec,25,zalog2,re2);
time3 =adjust_car_angle(vec,25,zalog3,re3);
tt2 = 40;
tt3 = 44+tt2;
fdelta = @(t) 0 + ... 
             dbeta*min(t-0,time(1)).*(t>0) - ...
             dbeta*min(t-time(1)-time(2),time(1)).*(t>( time(1)+time(2) ) )+...
             dbeta*min(t-tt2,time1(1)).*(t>tt2) - ...
             dbeta*min(t-time1(1)-time1(2)-tt2,time1(1)).*(t>( time1(1)+time1(2) +tt2  ) )-...
             dbeta*min(t-tt3,time3(1)).*(t>tt3) + ...
             dbeta*min(t-time3(1)-time3(2)-tt3,time3(1)).*(t>( time3(1)+time3(2) +tt3  ) );
%              dbeta*min(t-t2t,tt4).*(t>t2t)+...
%              dbeta*min(t-t2t-tt4,tt4).*(t>t2t+tt4)+...
%              dbeta*min(t-t2t-2*tt4,tt4).*(t>t2t+2*tt4)-...
%              dbeta*min(t-t2t+3*tt4,tt4).*(t>t2t+3*tt4)+...

% 后轴中点速度（由油门和刹车控制）
fv = @(t)min(t*2,vec);

% 求解微分方程的时间间隔和范围j
dt = 0.01;
t = [0:dt:120]';

% 车身角度 phi，车身中心坐标 (xc,yc) 的初始值 y0 = [phi0, xc0, yc0]
y0 = [zalog,6.53023,3.970426];

% 求解微分方程数值解，得到任意时刻车身的转角 phi(t) 和车身中心位置 (xc(t), yc(t))
[t,y] = ode45(@odecar, t, y0, [], fv, fdelta);
%[t,y] = run_the_car(@odecar, t, y0, fv, fdelta);
phi = y(:,1); xc = y(:,2); yc = y(:,3);



%wroterObj = VideoWriter('test.avi');
%open(wroterObj);

% 动态绘图
for i = 1:10:length(t)
    try
    plotcar(xc(i),yc(i),phi(i),fdelta(t(i)),h);
    disp(phi(i));
    set(h2, 'XData', xc(1:i),'YData', yc(1:i));
    drawnow;
   % frame = getframe;
   % writeVideo(wroterObj,frame);
    catch ErrorInfo 
     %   close(wroterObj);
       disp("get finish");
     
        break;
     
    end
    
end

figure
subplot(1,3,1); plot(t,fdelta(t)); xlabel('时间(s)'); ylabel('方向盘角度')
subplot(1,3,2); plot(t,fv(t));        xlabel('时间(s)'); ylabel('后车轮速度')
subplot(1,3,3); plot(t,phi);        xlabel('时间(s)'); ylabel('车身位')

